#include "path.h"

bool cmp(line a, line b)
{
    return a.dot_mid.l < b.dot_mid.l;
}

int path::get_position(my_msg::LaserScans* msg)
{
    vector<line> lines;
    int          len = msg->ranges.size();
    line*        t   = new line();
    /**************************************************************************
     * 将线段识别并添加进lines
     * 逻辑有点复杂，懒得写注释了
     */
    for ( unsigned long i = 0; i < len; i++ ) {
        double angle_t = -msg->angle_min - i * msg->angle_increment;
        if ( type == BACK ) {
            angle_t = angle_t > 0 ? angle_t - Pi : angle_t + Pi;
        }
        if ( msg->ranges[i] < (max_length) ) {
            if ( !t->dots.empty() ) {
                // 距离限制
                if ( abs(t->dots[t->dots.size() - 1].l - msg->ranges[i]) < 0.05 ) {
                    t->dots.emplace_back(msg->ranges[i], angle_t);
                }
                else {
                    lines.push_back(*t);
                    t = new line();
                    t->dots.emplace_back(msg->ranges[i], angle_t);
                }
            }
            else {
                t->dots.emplace_back(msg->ranges[i], angle_t);
            }
        }
        else {
            if ( t->dots.empty() ) {
                continue;
            }
            if ( abs(t->dots[t->dots.size() - 1].angle - angle_t) <=
                 3 * msg->angle_increment ) {
                continue;
            }
            else {
                lines.push_back(*t);
                t = new line();
            }
        }
    }
    if ( !t->dots.empty() ) {
        lines.push_back(*t);
    }
    if ( lines.empty() ) {
        return 0;
    }
    // 过零处理
    len = lines.size();
    if ( abs(lines[0].dots[0].l -
             lines[len - 1].dots[lines[len - 1].dots.size() - 1].l) < 0.1 &&
         lines[0].dots[0].angle > -1 * Pi / 10 &&
         lines[0].dots[0].angle < Pi / 100 &&
         lines[len - 1].dots[lines[len - 1].dots.size() - 1].angle < Pi / 10 &&
         lines[len - 1].dots[lines[len - 1].dots.size() - 1].angle >
             -1 * Pi / 100 ) {
        t = new line();
        for ( int i = lines[0].dots.size() - 1; i >= 0; i-- ) {
            t->dots.emplace_back(lines[0].dots[i].x, lines[0].dots[i].y, 1);
        }
        for ( int i = lines[len - 1].dots.size() - 1; i >= 0; i-- ) {
            t->dots.emplace_back(lines[len - 1].dots[i].x, lines[len - 1].dots[i].y, 1);
        }
        lines.erase(lines.begin());
        lines.erase(lines.end() - 1);
        lines.push_back(*t);
    }
    //**************************************************************************

    int t1 = count_side_sign(lines);

    return t1;
}

inline int path::count_side_sign(vector<line>& lines)
{
    vector<line> t_lines;
    dot          PL;
    for ( auto& line : lines ) {
        line.fill_line();
    }
    // 寻找在范围内的线段并进行填充
    for ( auto& line : lines ) {
        if ( line.dots[0].angle > -1 * Pi / 4 && line.dots.size() >= 5 && line.dot_mid.y > 0 ) {
            line.fill_line();
            t_lines.push_back(line);
        }
    }


    sort(t_lines.begin(), t_lines.end(), cmp);
    // 寻找y距离相差不多的两个线进行处理
    // 基本确定一定能找到，就省去了异常处理
    for ( int i = 0; i < t_lines.size(); i++ ) {
        if ( abs(t_lines[i].k) < 0.2 ) {
            if ( t_lines[i].dot_mid.l < 0.65 ) {
                return 2;
            }
        }
    }
}


/**
 * \name LINE
 * \brief
 * 成员函数定义
 */
void line::fill_line()
{
    unsigned long long n = dots.size();

    double sumX = 0.0, sumY = 0.0, sumXY = 0.0, sumX2 = 0.0;
    for ( auto& i : dots ) {
        sumX += i.x;
        sumY += i.y;
        sumXY += i.x * i.y;
        sumX2 += i.x * i.x;
    }

    k = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX);
    b = (sumY - k * sumX) / n;
    double x_mid =
        (b * tan(dots[0].angle) / (1 - k * tan(dots[0].angle)) +
         b * tan(dots[n - 1].angle) / (1 - k * tan(dots[n - 1].angle))) /
        2;

    dot_mid = dot(x_mid, k * x_mid + b, -1);
}
